// Copyright 2017 ETH Zurich and University of Bologna.
// Copyright and related rights are licensed under the Solderpad Hardware
// License, Version 0.51 (the “License”); you may not use this file except in
// compliance with the License.  You may obtain a copy of the License at
// http://solderpad.org/licenses/SHL-0.51. Unless required by applicable law
// or agreed to in writing, software, hardware and materials distributed under
// this License is distributed on an “AS IS” BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.

#include "stdio.h"
#include "stdarg.h"
// #include "string_lib.h"
//#include "utils.h"
#include "stdint.h"
#include "uart.h"


#define PAD_RIGHT 1
#define PAD_ZERO  2

#define	MAXNBUF	32
// #define va_arg   __crt_va_arg
#define	PCHAR(c) {(*func)(c, arg); retval++;}
static  int imax(int a, int b) { return (a > b ? a : b); }

/* the following should be enough for 32 bit int */
#define PRINT_BUF_LEN 32

/* define LONG_MAX for int32 */
#define LONG_MAX 2147483647L

/* DETECTNULL returns nonzero if (long)X contains a NULL byte. */
#if LONG_MAX == 2147483647L
#define DETECTNULL(X) (((X) - 0x01010101) & ~(X) & 0x80808080)
#else
#if LONG_MAX == 9223372036854775807L
#define DETECTNULL(X) (((X) - 0x0101010101010101) & ~(X) & 0x8080808080808080)
#else
#error long int is not a 32bit or 64bit type.
#endif
#endif

typedef	unsigned long long	u_quad_t;
typedef	long long   quad_t;

/* Nonzero if either X or Y is not aligned on a "long" boundary. */
#define UNALIGNED(X, Y) \
  (((long)X & (sizeof (long) - 1)) | ((long)Y & (sizeof (long) - 1)))

static unsigned divu10(unsigned n) {
  unsigned q, r;

  q = (n >> 1) + (n >> 2);
  q = q + (q >> 4);
  q = q + (q >> 8);
  q = q + (q >> 16);
  q = q >> 3;
  r = n - q * 10;

  return q + ((r + 6) >> 4);
}

char remu10_table[16] = {
  0, 1, 2, 2, 3, 3, 4, 5,
  5, 6, 7, 7, 8, 8, 9, 0
};

static unsigned remu10(unsigned n) {
  n = (0x19999999 * n + (n >> 1) + (n >> 3)) >> 28;
  return remu10_table[n];
}




int putchar(int s)
{
  uart_sendchar(s);
  return s;
}

static void qprintchar(char **str, int c)
{
  if (str)
  {
    **str = c;
    ++(*str);
  }
  else
    putchar((char)c);
}

static int qprints(char **out, const char *string, int width, int pad)
{
  register int pc = 0, padchar = ' ';

  if (width > 0) {
    register int len = 0;
    register const char *ptr;
    for (ptr = string; *ptr; ++ptr) ++len;
    if (len >= width) width = 0;
    else width -= len;
    if (pad & PAD_ZERO) padchar = '0';
  }
  if (!(pad & PAD_RIGHT)) {
    for ( ; width > 0; --width) {
      qprintchar (out, padchar);
      ++pc;
    }
  }
  for ( ; *string ; ++string) {
    qprintchar (out, *string);
    ++pc;
  }
  for ( ; width > 0; --width) {
    qprintchar (out, padchar);
    ++pc;
  }

  return pc;
}

static int qprinti(char **out, int i, int b, int sg, int width, int pad, char letbase)
{
  char print_buf[PRINT_BUF_LEN];
  register char *s;
  register int neg = 0, pc = 0;
  unsigned int t,u = i;

  if (i == 0)
  {
    print_buf[0] = '0';
    print_buf[1] = '\0';
    return qprints (out, print_buf, width, pad);
  }

  if (sg && b == 10 && i < 0)
  {
    neg = 1;
    u = -i;
  }

  s = print_buf + PRINT_BUF_LEN-1;
  *s = '\0';

  // treat HEX and decimal differently
  if(b == 16) {
    // HEX
    while (u) {
      int t = u & 0xF;

      if (t >= 10)
        t += letbase - '0' - 10;

      *--s = t + '0';
      u >>= 4;
    }
  } else {
    // decimal
    while (u) {
      *--s = remu10(u) + '0';
      u = divu10(u);
    }
  }

  if (neg) {
    if( width && (pad & PAD_ZERO) )
    {
      qprintchar (out, '-');
      ++pc;
      --width;
    }
    else
    {
      *--s = '-';
    }
  }
  return pc + qprints (out, s, width, pad);
}

static int qprint(char **out, const char *format, va_list va)
{
  register int width, pad;
  register int pc = 0;
  char scr[2];

  for (; *format != 0; ++format)
  {
    if (*format == '%')
    {
      ++format;
      width = pad = 0;
      if (*format == '\0') break;
      if (*format == '%') goto out;
      if (*format == '-')
      {
        ++format;
        pad = PAD_RIGHT;
      }
      while (*format == '0')
      {
        ++format;
        pad |= PAD_ZERO;
      }
      for ( ; *format >= '0' && *format <= '9'; ++format) {
        width *= 10;
        width += *format - '0';
      }
      if( *format == 's' ) {
        register char *s = va_arg(va, char*);
        pc += qprints (out, s?s:"(null)", width, pad);
        continue;
      }
      if( *format == 'd' ) {
        pc += qprinti (out, va_arg(va, int), 10, 1, width, pad, 'a');
        continue;
      }
      if( *format == 'u' ) {
        pc += qprinti (out, va_arg(va, unsigned int), 10, 0, width, pad, 'a');
        continue;
      }
      if( *format == 'x' ) {
        pc += qprinti (out, va_arg(va, uint32_t), 16, 0, width, pad, 'a');
        continue;
      }
      if( *format == 'X' ) {
        pc += qprinti (out, va_arg(va, uint32_t), 16, 0, width, pad, 'A');
        continue;
      }
      if( *format == 'c' ) {
        scr[0] = va_arg(va, int);
        scr[1] = '\0';
        pc += qprints (out, scr, width, pad);
        continue;
      }
    }
    else
    {
out:
      qprintchar (out, *format);
      ++pc;
    }
  }
  if (out) **out = '\0';

  return pc;
}

int printf(const char *format, ...)
{
  int pc;
  va_list va;

  va_start(va, format);

  pc = qprint(0, format, va);

  va_end(va);

  return pc;

}

int puts(const char *s)
{
  int i=0;

  while(s[i] != '\0')
    putchar(s[i++]);

  putchar('\n');

  return i;
}

int strcmp (const char *s1, const char *s2)
{
  unsigned long *a1;
  unsigned long *a2;

  /* If s1 or s2 are unaligned, then compare bytes. */
  if (!UNALIGNED (s1, s2))
  {
    /* If s1 and s2 are word-aligned, compare them a word at a time. */
    a1 = (unsigned long*)s1;
    a2 = (unsigned long*)s2;
    while (*a1 == *a2)
      {
        /* To get here, *a1 == *a2, thus if we find a null in *a1,
           then the strings must be equal, so return zero.  */
          if (DETECTNULL (*a1))
          return 0;
        a1++;
        a2++;
      }
    /* A difference was detected in last few bytes of s1, so search bytewise */
    s1 = (char*)a1;
    s2 = (char*)a2;
  }

  while (*s1 != '\0' && *s1 == *s2)
  {
    s1++;
    s2++;
  }
  return (*(unsigned char *) s1) - (*(unsigned char *) s2);
}

int strncmp(const char *s1, const char *s2, size_t len)
{
    while(--len && *s1 && *s2 && (*s1==*s2)) s1++, s2++;
    
    return (*s1-*s2);
}



void * memset (void *dest, int val, size_t length)
{
  unsigned char *ptr = dest;
  while (length > 0) {
    *ptr++ = val;
    length --;
  }
  return dest;
}

char* strcpy (char *s1, const char *s2)
{
  char *s1p = (char *)s1;
  char *s2p = (char *)s2;

  while (*s2p != '\0')
  {
    (*s1p) = (*s2p);

    ++s1p;
    ++s2p;
  }

  return s1;
}

size_t strlen (const char *str)
{
  char *s = (char *)str;
  size_t len = 0;

  if (s == NULL)
    return 0;

  while (*s++ != '\0')
    ++len;
  return len;
}

/*
 * Put a NUL-terminated ASCII number (base <= 36) in a buffer in reverse
 * order; return an optional length and a pointer to the last character
 * written in the buffer (i.e., the first character of the string).
 * The buffer pointed to by `nbuf' must have length >= MAXNBUF.
 */
// static char *
// pn(char *nbuf, uintmax_t num, int base, int *lenp, int upper)
// {
// 	char *p, c;

// 	p = nbuf;
// 	*p = '\0';
// 	do {
// 		c = num % base;
// 		num /= base;
// 		if (c < 10)
//                 	c += '0';
// 		else if (upper)
//                 	c += 'A' - 10;
// 		else
//                 	c += 'a' - 10;
// 		*++p = c;
// 	} while (num != 0);
// 	if (lenp)
// 		*lenp = p - nbuf;
// 	return (p);
// }

// static int
// pd(void(*func)(int, void *), void *arg, double x)
// {
// 	double tx, one = 1.0, integ = 0, lim = 0.00000000001;
// 	char cb[128]; /* XXX hardcoded buffer size on stack - revisit!!! */
// 	int first = 0, last = 0, retval = 0, i, t;

// 	if (x == 0) {
// 		PCHAR('0');
// 		return(retval);
// 	}

// 	if (x < 0) {
// 		x = -x;
// 		PCHAR('-');
// 	}

// 	if (x < one) {
// 		PCHAR('0');
// 	} else {
// 		for (tx = x; tx >= one; tx /= 10, lim *= 10) {
// 			i = tx;
// 			i %= 10;
// 			cb[last++] = '0' + i;
// 		}
// 		if (last > first)
// 			last--;
// 		for (i = 0; i < (last - first + 1) / 2; i++) {
// 			t = cb[first + i];
// 			cb[first + i] = cb[last - i];
// 			cb[last - i] = t;
// 		}
// 		for (i = first; i <= last; i++) {
// 			PCHAR(cb[i]);
// 			integ = integ * 10 + (cb[i] - '0');
// 		}
// 		x = x - integ;
// 		if (x < lim)
// 			return(retval);
// 	}

// 	tx = x;
// 	PCHAR('.');
// 	while (integ == 0 || x > lim) {
// 		tx = tx * 10.0;
// 		x = x / 10.0;
// 		i = tx;
// 		if (integ == 0 && i == 0)
// 			lim /= 100;
// 		else
// 			integ = 1;
// 		PCHAR('0' + (i % 10));
// 	}
// 	return(retval);
// }


// int
// _xvprintf(char const *fmt, void(*func)(int, void *), void *arg, va_list ap)
// {
// 	char nbuf[MAXNBUF];
// 	const char *p, *percent;
// 	int ch, n;
// 	uintmax_t num;
// 	int base, lflag, qflag, tmp, width, ladjust, sharpflag, neg, sign, dot;
// 	int cflag, hflag, jflag, zflag;
// 	int dwidth, upper;
// 	char padc;
// 	int stop = 0, retval = 0;

// 	num = 0;
//     printf("in xvprintf\r\n");
//     printf("xvin retval= %d\r\n",retval);

// 	if (fmt == NULL)
// 		fmt = "(fmt null)\n";

// 	for (;;) {
// 		padc = ' ';
// 		width = 0;
// 		while ((ch = (u_char)*fmt++) != '%' || stop) {
//             printf("xvre retval= %d\r\n",retval);
// 			if (ch == '\0')
// 				return (retval);
// 			PCHAR(ch);
// 		}
// 		percent = fmt - 1;
// 		qflag = 0; lflag = 0; ladjust = 0; sharpflag = 0; neg = 0;
// 		sign = 0; dot = 0; dwidth = 0; upper = 0;
// 		cflag = 0; hflag = 0; jflag = 0; zflag = 0;
// reswitch:	switch (ch = (unsigned char)*fmt++) {
// 		case '.':
// 			dot = 1;
// 			goto reswitch;
// 		case '#':
// 			sharpflag = 1;
// 			goto reswitch;
// 		case '+':
// 			sign = 1;
// 			goto reswitch;
// 		case '-':
// 			ladjust = 1;
// 			goto reswitch;
// 		case '%':
// 			PCHAR(ch);
//             // printf("xv1 retval= %d\r\n",retval);
// 			break;
// 		case '*':
// 			if (!dot) {
// 				width = va_arg(ap, int);
// 				if (width < 0) {
// 					ladjust = !ladjust;
// 					width = -width;
// 				}
// 			} else {
// 				dwidth = va_arg(ap, int);
// 			}
// 			goto reswitch;
// 		case '0':
// 			if (!dot) {
// 				padc = '0';
// 				goto reswitch;
// 			}
// 		case '1': case '2': case '3': case '4':
// 		case '5': case '6': case '7': case '8': case '9':
// 				for (n = 0;; ++fmt) {
// 					n = n * 10 + ch - '0';
// 					ch = *fmt;
// 					if (ch < '0' || ch > '9')
// 						break;
// 				}
// 			if (dot)
// 				dwidth = n;
// 			else
// 				width = n;
// 			goto reswitch;
// 		case 'c':
// 			PCHAR(va_arg(ap, int));
//             // printf("xv2 retval= %d\r\n",retval);
// 			break;
// 		case 'd':
// 		case 'i':
// 			base = 10;
// 			sign = 1;
// 			goto handle_sign;
// 		case 'h':
// 			if (hflag) {
// 				hflag = 0;
// 				cflag = 1;
// 			} else
// 				hflag = 1;
// 			goto reswitch;
// 		case 'j':
// 			jflag = 1;
// 			goto reswitch;
// 		case 'l':
// 			if (lflag) {
// 				lflag = 0;
// 				qflag = 1;
// 			} else
// 				lflag = 1;
// 			goto reswitch;
// 		case 'n':
// 			if (jflag)
// 				*(va_arg(ap, intmax_t *)) = retval;
// 			else if (qflag)
// 				*(va_arg(ap, quad_t *)) = retval;
// 			else if (lflag)
// 				*(va_arg(ap, long *)) = retval;
// 			else if (zflag)
// 				*(va_arg(ap, size_t *)) = retval;
// 			else if (hflag)
// 				*(va_arg(ap, short *)) = retval;
// 			else if (cflag)
// 				*(va_arg(ap, char *)) = retval;
// 			else
// 				*(va_arg(ap, int *)) = retval;
// 			break;
// 		case 'o':
// 			base = 8;
// 			goto handle_nosign;
// 		case 'p':
// 			base = 16;
// 			sharpflag = (width == 0);
// 			sign = 0;
// 			num = (uintptr_t)va_arg(ap, void *);
// 			goto number;
// 		case 'q':
// 			qflag = 1;
// 			goto reswitch;
// 		case 's':
// 			p = va_arg(ap, char *);
// 			if (p == NULL)
// 				p = "(null)";
// 			if (!dot)
// 				n = strlen (p);
// 			else
// 				for (n = 0; n < dwidth && p[n]; n++)
// 					continue;

// 			width -= n;

// 			if (!ladjust && width > 0)
// 				while (width--){
// 					PCHAR(padc);
//                     // printf("xv3 retval= %d\r\n",retval);
//                     }
// 			while (n--){
// 				PCHAR(*p++);
//                 printf("xv4 retval= %d\r\n",retval);
//                 }
// 			if (ladjust && width > 0)
// 				while (width--){
// 					PCHAR(padc);
//                     // printf("xv5 retval= %d\r\n",retval);
//                     }
// 			break;
// 		case 'u':
// 			base = 10;
// 			goto handle_nosign;
// 		case 'X':
// 			upper = 1;
// 		case 'x':
// 			base = 16;
// 			goto handle_nosign;
// 		case 'y':
// 			base = 16;
// 			sign = 1;
// 			goto handle_sign;
// 		case 'z':
// 			zflag = 1;
// 			goto reswitch;
// handle_nosign:
// 			sign = 0;
// 			if (jflag)
// 				num = va_arg(ap, uintmax_t);
// 			else if (qflag)
// 				num = va_arg(ap, u_quad_t);
// 			else if (lflag)
// 				num = va_arg(ap, u_long);
// 			else if (zflag)
// 				num = va_arg(ap, size_t);
// 			else if (hflag)
// 				num = (u_short)va_arg(ap, int);
// 			else if (cflag)
// 				num = (u_char)va_arg(ap, int);
// 			else
// 				num = va_arg(ap, u_int);
// 			goto number;
// handle_sign:
// 			if (jflag)
// 				num = va_arg(ap, intmax_t);
// 			else if (qflag)
// 				num = va_arg(ap, quad_t);
// 			else if (lflag)
// 				num = va_arg(ap, long);
// 			else if (zflag)
// 				num = va_arg(ap, ssize_t);
// 			else if (hflag)
// 				num = (short)va_arg(ap, int);
// 			else if (cflag)
// 				num = (char)va_arg(ap, int);
// 			else
// 				num = va_arg(ap, int);
// number:
// 			if (sign && (intmax_t)num < 0) {
// 				neg = 1;
// 				num = -(intmax_t)num;
// 			}
// 			p = pn(nbuf, num, base, &n, upper);
// 			tmp = 0;
// 			if (sharpflag && num != 0) {
// 				if (base == 8)
// 					tmp++;
// 				else if (base == 16)
// 					tmp += 2;
// 			}
// 			if (neg)
// 				tmp++;

// 			if (!ladjust && padc == '0')
// 				dwidth = width - tmp;
// 			width -= tmp + imax(dwidth, n);
// 			dwidth -= n;
// 			if (!ladjust)
// 				while (width-- > 0){
// 					PCHAR(' ');
//                     // printf("xv6 retval= %d\r\n",retval);
//                     }
// 			if (neg){
// 				PCHAR('-');
//                 // printf("xv7 retval= %d\r\n",retval);
//                 }
// 			if (sharpflag && num != 0) {
// 				if (base == 8) {
// 					PCHAR('0');
//                     // printf("xv8 retval= %d\r\n",retval);
// 				} else if (base == 16) {
// 					PCHAR('0');
// 					PCHAR('x');
//                     // printf("xv9 retval= %d\r\n",retval);
// 				}
// 			}
// 			while (dwidth-- > 0){
// 				PCHAR('0');
//                 printf("xv10 retval= %d\r\n",retval);
//                 }
// 			while (*p){
// 				PCHAR(*p--);
//                 printf("xv11 retval= %d\r\n",retval);
//                 }
// 			if (ladjust)
// 				while (width-- > 0){
// 					PCHAR(' ');
//                     printf("xv12 retval= %d\r\n",retval);
//                     }
// 			break;
// #ifndef NO_PRINTF_FLOAT
// 		case 'f':
// 			retval += pd(func, arg, va_arg(ap, double));
//             // printf("xv14 retval= %d\r\n",retval);
// 			break;
// #endif /* NO_PRINTF_FLOAT */
// 		default:
// 			while (percent < fmt){
// 				PCHAR(*percent++);
//                 printf("xv13 retval= %d\r\n",retval);
// 			/*
// 			 * Since we ignore a formatting argument it is no 
// 			 * longer safe to obey the remaining formatting
// 			 * arguments as the arguments will no longer match
// 			 * the format specs.
// 			 */
// 			stop = 1;
// 			break;}
// 		}
// 	}
// }

// struct snprintf_arg {
// 	char    *str;
// 	size_t  remain;
// };


// static void
// snprintf_pchar(int c, void *arg)
// {
// 	struct snprintf_arg *const info = arg;

// 	if (info->remain != 1)
// 		*info->str++ = c;
// 	if (info->remain >= 2)
// 		info->remain--;
// }


//  int
// vsnprintf(char *str, size_t size, const char *fmt, va_list ap)
// {
// 	struct snprintf_arg info;
// 	int retval;
//     printf("in vsnprintf!\r\n");
// 	info.str = str;
// 	info.remain = size;
// 	retval = _xvprintf(fmt, snprintf_pchar, &info, ap);
//     printf("%s\r\n",str);
//     printf("vsn retval= %d\r\n",retval);
// 	if (info.remain >= 1)
// 		*info.str++ = '\0';
// 	return (retval);
// }
